% dLdq_dot = collect(simplify(expand(diff(L,q_dot))),[q_dot theta_dot_1 theta_dot_2])
% syms q_ddot theta_ddot_1 theta_ddot_2
% Qq = (m_q + m1 + m2)*q_ddot + ...
% + l1*m1*(theta_ddot_1*cos(theta_1) + theta_dot_1*(-sin(theta_1)*theta_dot_1)) ...
% + l1*m2*(theta_ddot_1*cos(theta_1) + theta_dot_1*(-sin(theta_1)*theta_dot_1)) ...
% + l2*m2*(theta_ddot_2*cos(theta_2) + theta_dot_2*(-sin(theta_2)*theta_dot_2));
% Qq = simplify(expand(Qq))
% dLdtheta_dot_1 = collect(simplify(expand(diff(L,theta_dot_1))),[q_dot theta_dot_1 theta_dot_2])
% dLdtheta_dot_1_dt = q_ddot*l1*(m1*cos(theta_1) + m2*cos(theta_2)) ...
% + q_dot*l1*(m1*(-sin(theta_1)*theta_dot_1) + m2*(-sin(theta_2)*theta_dot_2)) ...
% + theta_ddot_1*(l1^2*(m1+m2)) ...
% + theta_ddot_2*l1*l2*m2*cos(theta_1-theta_2) ...
% + theta_dot_2*l1*l2*m2*(-sin(theta_1-theta_2)*(theta_dot_1 - theta_dot_2))
% dLdtheta_1 = collect(simplify(expand(diff(L,theta_1))),[q_dot,theta_dot_1, theta_dot_2])
% Q1 = collect(simplify(expand(dLdtheta_dot_1_dt - dLdtheta_1)),[q_ddot, theta_ddot_1, theta_ddot_2, theta_dot_2,g])
dLdq_dot = collect(simplify(expand(diff(L,diff(q,t)))),[diff(q,t) diff(theta_1,t) diff(theta_2,t)]);
ddt_dLdq_dot = diff(dLdq_dot,t);
Q0 = collect(simplify(expand(ddt_dLdq_dot - dLdq)),[diff(q,t,2), diff(theta_1,t,2), diff(theta_1,t), diff(theta_2,t,2), diff(theta_2,t)])
Q0(t) =

dLdtheta_1_dot = collect(simplify(expand(diff(L,diff(theta_1,t)))),[diff(q,t) diff(theta_1,t) diff(theta_2,t)]);
ddt_dLdtheta_1_dot = diff(dLdtheta_1_dot,t);
dLdtheta_1 = diff(L,theta_1);
Q1 = collect(simplify(expand(ddt_dLdtheta_1_dot - dLdtheta_1)),[diff(q,t,2), diff(theta_1,t,2), diff(theta_1,t), diff(theta_2,t,2), diff(theta_2,t)])
Q1(t) =

dLdtheta_2_dot = collect(simplify(expand(diff(L,diff(theta_2,t)))),[diff(q,t) diff(theta_1,t) diff(theta_2,t)]);
ddt_dLdtheta_2_dot = diff(dLdtheta_2_dot,t);
dLdtheta_2 = diff(L,theta_2);
Q2 = collect(simplify(expand(ddt_dLdtheta_2_dot - dLdtheta_2)),[diff(q,t,2), diff(theta_1,t,2), diff(theta_1,t), diff(theta_2,t,2), diff(theta_2,t)])
Q2(t) =

syms q_ddot theta_ddot_1 theta_ddot_2 % creating symbolic variables for 2nd derivatives
eqn0 = subs(Q0 == u + w0 - d0*diff(q,t), [diff(q,t,2), diff(theta_1,t,2), diff(theta_2,t,2)], [q_ddot, theta_ddot_1, theta_ddot_2]);
eqn1 = subs(Q1 == w1 - d1*diff(theta_1,t), [diff(q,t,2), diff(theta_1,t,2), diff(theta_2,t,2)], [q_ddot, theta_ddot_1, theta_ddot_2]);
eqn2 = subs(Q2 == w2 - d2*diff(theta_2,t), [diff(q,t,2), diff(theta_1,t,2), diff(theta_2,t,2)], [q_ddot, theta_ddot_1, theta_ddot_2]);
sol = solve([eqn0, eqn1, eqn2],[q_ddot, theta_ddot_1, theta_ddot_2]);
vars = symvar(A0+B0)
vars = 
% [1/s] friction damping coefficient
k = 1; % fraction of l2/l1
vars_real = [d0_real, d1_real, d2_real, g_real, l1_real, l2_real, m1_real, m2_real, m_q_real];
A0_real = subs(A0,vars,vars_real)
A0_real =

A0_real = double(A0_real);
B0_real = subs(B0,vars,vars_real)
B0_real =

B0_real = double(B0_real);
C = [eye(3) zeros(3,3)]; D = zeros(3,1);
rank(ctrb(A0_real,B0_real))
olsys = ss(A0_real,B0_real,C,D);
oltf = C*inv(s*eye(6) - A0_real)*B0_real;
% placing poles with 4 poles optimally damped and 1 set of poles critically damped
% speed of the poles chosen to balance damping with speed
p = [-5, -5, -10*(1 + 1i*[1, -1]), -10.6*(1 + 1i*[1, -1])];
gains = acker(A0_real,B0_real,p)
58.4965 -197.6694 286.9746 39.1935 0.8044 14.8938
% u = -Kx + r => xdot = (A - B*K)*x + B*r
Abar = A0_real-B0_real*gains;
states = {'x' 'theta_1' 'theta_1' 'x_dot' 'theta_dot_1' 'theta_dot_2'};
outputs = {'x'; 'theta_1'; 'theta_2'};
clsys = ss(Abar,B0_real,C,D,'statename',states,'inputname',inputs,'outputname',outputs);
initial(clsys,[0.05; -10*pi/180; -2*pi/180; 0; 0; 0])
% [1/s] friction damping coefficient
k = [0.1, 0.5, 0.75, 1, 5, 10, 50, 100]; % logspace(-2,3,10);
vars_iter = zeros(size(vars,1), size(vars,2),length(k));
A0_iter = zeros(6,6,length(k));
B0_iter = zeros(6,1,length(k));
gains = zeros(1,6,length(k));
Abar = zeros(size(A0_iter));
states = {'x' 'theta_1' 'theta_1' 'x_dot' 'theta_dot_1' 'theta_dot_2'};
outputs = {'x'; 'theta_1'; 'theta_2'};
vars_iter(:,:,i) = [d0_real, d1_real, d2_real, g_real, l1_real, l2_iter(i), m1_real, m2_real, m_q_real];
A0_iter(:,:,i) = double(subs(A0,vars,vars_iter(:,:,i)));
B0_iter(:,:,i) = double(subs(B0,vars,vars_iter(:,:,i)));
% placing poles at the same locations... will see if that has an effect
gains(:,:,i) = acker(A0_iter(:,:,i),B0_iter(:,:,i),p);
% u = -Kx + r => xdot = (A - B*K)*x + B*r
Abar(:,:,i) = A0_iter(:,:,i)-B0_iter(:,:,i)*gains(:,:,i);
clsys_iter(:,:,i) = ss(Abar(:,:,i),B0_iter(:,:,i),C,D,'statename',states,'inputname',inputs,'outputname',outputs);
[y(:,:,i), ~, x_iter(:,:,i)] = initial(clsys_iter(:,:,i),[0.05; -10*pi/180; -2*pi/180; 0; 0; 0],t);
ax1 = nexttile; hold on; grid on;
ax1.ColorOrder = [1 0 0; 0 0 1; 0 0.7 0; 0 0 0; 0.929 0.694 0.125];
ax1.LineStyleOrder = {'-','--'};
ax2 = nexttile; hold on; grid on;
ax2.ColorOrder = [1 0 0; 0 0 1; 0 0.7 0; 0 0 0; 0.929 0.694 0.125];
ax2.LineStyleOrder = {'-','--'};
ax3 = nexttile; hold on; grid on;
ax3.ColorOrder = [1 0 0; 0 0 1; 0 0.7 0; 0 0 0; 0.929 0.694 0.125];
ax3.LineStyleOrder = {'-','--'};
title(ax1,'Response to Initial Conditions');
ylabel(ax2,'To: \theta_1 [deg]');
ylabel(ax3,'To: \theta_2 [deg]'); xlabel('Time (seconds)');
plot(ax2,t,y(:,2,i)*180/pi);
plot(ax3,t,y(:,3,i)*180/pi);